using System;
using System.Collections;
using LightCAD.MathLib;

namespace LightCAD.MathLib
{
    public class Vector4 : Vector, IIOArray
    {
        #region Properties

        public double X;
        public double Y;
        public double Z;
        public double W;

        #endregion

        #region constructor
        public Vector4() : this(0, 0, 0, 0)
        {

        }
        public Vector4(double x = 0, double y = 0, double z = 0, double w = 1)
        {
            this.X = x;
            this.Y = y;
            this.Z = z;
            this.W = w;
        }
        #endregion

        #region properties
        public double Width
        {
            get
            {
                return this.Z;
            }
            set
            {
                this.Z = value;
            }
        }
        public double Height
        {
            get
            {
                return this.W;
            }
            set
            {
                this.W = value;
            }
        }
        #endregion

        #region methods
        public Vector4 Set(double x, double y, double z, double w)
        {
            this.X = x;
            this.Y = y;
            this.Z = z;
            this.W = w;
            return this;
        }
        public Vector4 SetScalar(double scalar)
        {
            this.X = scalar;
            this.Y = scalar;
            this.Z = scalar;
            this.W = scalar;
            return this;
        }
        public Vector4 SetX(double x)
        {
            this.X = x;
            return this;
        }
        public Vector4 SetY(double y)
        {
            this.Y = y;
            return this;
        }
        public Vector4 SetZ(double z)
        {
            this.Z = z;
            return this;
        }
        public Vector4 SetW(double w)
        {
            this.W = w;
            return this;
        }
        public Vector4 SetComponent(double index, double value)
        {
            switch (index)
            {
                case 0: this.X = value; break;
                case 1: this.Y = value; break;
                case 2: this.Z = value; break;
                case 3: this.W = value; break;
                default: throw new Error("index is out of range: " + index);
            }
            return this;
        }
        public double GetComponent(double index)
        {
            switch (index)
            {
                case 0: return this.X;
                case 1: return this.Y;
                case 2: return this.Z;
                case 3: return this.W;
                default: throw new Error("index is out of range: " + index);
            }
        }
        public Vector4 Clone()
        {
            return new Vector4(this.X, this.Y, this.Z, this.W);
        }
        public Vector4 Copy(Vector4 v)
        {
            this.X = v.X;
            this.Y = v.Y;
            this.Z = v.Z;
            this.W = (v.W != double.NaN) ? v.W : 1;
            return this;
        }
        public Vector4 Add(Vector4 v)
        {
            this.X += v.X;
            this.Y += v.Y;
            this.Z += v.Z;
            this.W += v.W;
            return this;
        }
        public Vector4 AddScalar(double s)
        {
            this.X += s;
            this.Y += s;
            this.Z += s;
            this.W += s;
            return this;
        }
        public Vector4 AddVectors(Vector4 a, Vector4 b)
        {
            this.X = a.X + b.X;
            this.Y = a.Y + b.Y;
            this.Z = a.Z + b.Z;
            this.W = a.W + b.W;
            return this;
        }
        public Vector4 AddScaledVector(Vector4 v, double s)
        {
            this.X += v.X * s;
            this.Y += v.Y * s;
            this.Z += v.Z * s;
            this.W += v.W * s;
            return this;
        }
        public Vector4 Sub(Vector4 v)
        {
            this.X -= v.X;
            this.Y -= v.Y;
            this.Z -= v.Z;
            this.W -= v.W;
            return this;
        }
        public Vector4 SubScalar(double s)
        {
            this.X -= s;
            this.Y -= s;
            this.Z -= s;
            this.W -= s;
            return this;
        }
        public Vector4 SubVectors(Vector4 a, Vector4 b)
        {
            this.X = a.X - b.X;
            this.Y = a.Y - b.Y;
            this.Z = a.Z - b.Z;
            this.W = a.W - b.W;
            return this;
        }
        public Vector4 Multiply(Vector4 v)
        {
            this.X *= v.X;
            this.Y *= v.Y;
            this.Z *= v.Z;
            this.W *= v.W;
            return this;
        }
        public Vector4 MultiplyScalar(double scalar)
        {
            this.X *= scalar;
            this.Y *= scalar;
            this.Z *= scalar;
            this.W *= scalar;
            return this;
        }
        public Vector4 ApplyMatrix4(Matrix4 m)
        {
            double x = this.X, y = this.Y, z = this.Z, w = this.W;
            double[] e = m.Elements;
            this.X = e[0] * x + e[4] * y + e[8] * z + e[12] * w;
            this.Y = e[1] * x + e[5] * y + e[9] * z + e[13] * w;
            this.Z = e[2] * x + e[6] * y + e[10] * z + e[14] * w;
            this.W = e[3] * x + e[7] * y + e[11] * z + e[15] * w;
            return this;
        }
        public Vector4 DivideScalar(double scalar)
        {
            return this.MultiplyScalar(1 / scalar);
        }
        public Vector4 SetAxisAngleFromQuaternion(Quaternion q)
        {
            // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/index.htm
            // q is assumed to be normalized
            this.W = 2 * Math.Acos(q.W);
            double s = Math.Sqrt(1 - q.W * q.W);
            if (s < 0.0001)
            {
                this.X = 1;
                this.Y = 0;
                this.Z = 0;
            }
            else
            {
                this.X = q.X / s;
                this.Y = q.Y / s;
                this.Z = q.Z / s;
            }
            return this;
        }
        public Vector4 SetAxisAngleFromRotationMatrix(Matrix4 m)
        {
            // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/index.htm
            // assumes the upper 3x3 of m is a pure rotation matrix (i.e, unscaled)
            double angle, x, y, z; // variables for result
            double epsilon = 0.01,      // margin to allow for rounding errors
                epsilon2 = 0.1;     // margin to distinguish between 0 and 180 degrees
            double[] te = m.Elements;
            double m11 = te[0], m12 = te[4], m13 = te[8],
                m21 = te[1], m22 = te[5], m23 = te[9],
                m31 = te[2], m32 = te[6], m33 = te[10];
            if ((Math.Abs(m12 - m21) < epsilon) &&
                 (Math.Abs(m13 - m31) < epsilon) &&
                 (Math.Abs(m23 - m32) < epsilon))
            {
                // singularity found
                // first check for identity matrix which must have +1 for all terms
                // in leading diagonal and zero in other terms
                if ((Math.Abs(m12 + m21) < epsilon2) &&
                     (Math.Abs(m13 + m31) < epsilon2) &&
                     (Math.Abs(m23 + m32) < epsilon2) &&
                     (Math.Abs(m11 + m22 + m33 - 3) < epsilon2))
                {
                    // this singularity is identity matrix so angle = 0
                    this.Set(1, 0, 0, 0);
                    return this; // zero angle, arbitrary axis
                }
                // otherwise this singularity is angle = 180
                angle = MathEx.PI;
                double xx = (m11 + 1) / 2;
                double yy = (m22 + 1) / 2;
                double zz = (m33 + 1) / 2;
                double xy = (m12 + m21) / 4;
                double xz = (m13 + m31) / 4;
                double yz = (m23 + m32) / 4;
                if ((xx > yy) && (xx > zz))
                {
                    // m11 is the largest diagonal term
                    if (xx < epsilon)
                    {
                        x = 0;
                        y = 0.707106781;
                        z = 0.707106781;
                    }
                    else
                    {
                        x = Math.Sqrt(xx);
                        y = xy / x;
                        z = xz / x;
                    }
                }
                else if (yy > zz)
                {
                    // m22 is the largest diagonal term
                    if (yy < epsilon)
                    {
                        x = 0.707106781;
                        y = 0;
                        z = 0.707106781;
                    }
                    else
                    {
                        y = Math.Sqrt(yy);
                        x = xy / y;
                        z = yz / y;
                    }
                }
                else
                {
                    // m33 is the largest diagonal term so base result on this
                    if (zz < epsilon)
                    {
                        x = 0.707106781;
                        y = 0.707106781;
                        z = 0;
                    }
                    else
                    {
                        z = Math.Sqrt(zz);
                        x = xz / z;
                        y = yz / z;
                    }
                }
                this.Set(x, y, z, angle);
                return this; // return 180 deg rotation
            }
            // as we have reached here there are no singularities so we can handle normally
            double s = Math.Sqrt((m32 - m23) * (m32 - m23) +
                                (m13 - m31) * (m13 - m31) +
                                (m21 - m12) * (m21 - m12)); // used to Normalize
            if (Math.Abs(s) < 0.001) s = 1;
            // prevent divide by zero, should not happen if matrix is orthogonal and should be
            // caught by singularity test above, but I"ve left it in just in case
            this.X = (m32 - m23) / s;
            this.Y = (m13 - m31) / s;
            this.Z = (m21 - m12) / s;
            this.W = Math.Acos((m11 + m22 + m33 - 1) / 2);
            return this;
        }
        public Vector4 Min(Vector4 v)
        {
            this.X = Math.Min(this.X, v.X);
            this.Y = Math.Min(this.Y, v.Y);
            this.Z = Math.Min(this.Z, v.Z);
            this.W = Math.Min(this.W, v.W);
            return this;
        }
        public Vector4 Max(Vector4 v)
        {
            this.X = Math.Max(this.X, v.X);
            this.Y = Math.Max(this.Y, v.Y);
            this.Z = Math.Max(this.Z, v.Z);
            this.W = Math.Max(this.W, v.W);
            return this;
        }
        public Vector4 Clamp(Vector4 min, Vector4 max)
        {
            // assumes min < max, componentwise
            this.X = Math.Max(min.X, Math.Min(max.X, this.X));
            this.Y = Math.Max(min.Y, Math.Min(max.Y, this.Y));
            this.Z = Math.Max(min.Z, Math.Min(max.Z, this.Z));
            this.W = Math.Max(min.W, Math.Min(max.W, this.W));
            return this;
        }
        public Vector4 ClampScalar(double minVal, double maxVal)
        {
            this.X = Math.Max(minVal, Math.Min(maxVal, this.X));
            this.Y = Math.Max(minVal, Math.Min(maxVal, this.Y));
            this.Z = Math.Max(minVal, Math.Min(maxVal, this.Z));
            this.W = Math.Max(minVal, Math.Min(maxVal, this.W));
            return this;
        }
        public Vector4 ClampLength(double min, double max)
        {
            double length = this.Length();
            return this.DivideScalar(length == 0 ? 1 : length).MultiplyScalar(Math.Max(min, Math.Min(max, length)));
        }
        public Vector4 Floor()
        {
            this.X = Math.Floor(this.X);
            this.Y = Math.Floor(this.Y);
            this.Z = Math.Floor(this.Z);
            this.W = Math.Floor(this.W);
            return this;
        }
        public Vector4 Ceil()
        {
            this.X = Math.Ceiling(this.X);
            this.Y = Math.Ceiling(this.Y);
            this.Z = Math.Ceiling(this.Z);
            this.W = Math.Ceiling(this.W);
            return this;
        }
        public Vector4 Round()
        {
            this.X = Math.Round(this.X);
            this.Y = Math.Round(this.Y);
            this.Z = Math.Round(this.Z);
            this.W = Math.Round(this.W);
            return this;
        }
        public Vector4 RoundToZero()
        {
            this.X = (this.X < 0) ? Math.Ceiling(this.X) : Math.Floor(this.X);
            this.Y = (this.Y < 0) ? Math.Ceiling(this.Y) : Math.Floor(this.Y);
            this.Z = (this.Z < 0) ? Math.Ceiling(this.Z) : Math.Floor(this.Z);
            this.W = (this.W < 0) ? Math.Ceiling(this.W) : Math.Floor(this.W);
            return this;
        }
        public Vector4 Negate()
        {
            this.X = -this.X;
            this.Y = -this.Y;
            this.Z = -this.Z;
            this.W = -this.W;
            return this;
        }
        public double Dot(Vector4 v)
        {
            return this.X * v.X + this.Y * v.Y + this.Z * v.Z + this.W * v.W;
        }
        public double LengthSq()
        {
            return this.X * this.X + this.Y * this.Y + this.Z * this.Z + this.W * this.W;
        }
        public double Length()
        {
            return Math.Sqrt(this.X * this.X + this.Y * this.Y + this.Z * this.Z + this.W * this.W);
        }
        public double ManhattanLength()
        {
            return Math.Abs(this.X) + Math.Abs(this.Y) + Math.Abs(this.Z) + Math.Abs(this.W);
        }
        public Vector4 Normalize()
        {
            var len = this.Length();
            return this.DivideScalar(len == 0 ? 1 : len);
        }
        public Vector4 SetLength(double length)
        {
            return this.Normalize().MultiplyScalar(length);
        }
        public Vector4 Lerp(Vector4 v, double alpha)
        {
            this.X += (v.X - this.X) * alpha;
            this.Y += (v.Y - this.Y) * alpha;
            this.Z += (v.Z - this.Z) * alpha;
            this.W += (v.W - this.W) * alpha;
            return this;
        }
        public Vector4 LerpVectors(Vector4 v1, Vector4 v2, double alpha)
        {
            this.X = v1.X + (v2.X - v1.X) * alpha;
            this.Y = v1.Y + (v2.Y - v1.Y) * alpha;
            this.Z = v1.Z + (v2.Z - v1.Z) * alpha;
            this.W = v1.W + (v2.W - v1.W) * alpha;
            return this;
        }
        public bool Equals(Vector4 v)
        {
            return ((v.X == this.X) && (v.Y == this.Y) && (v.Z == this.Z) && (v.W == this.W));
        }
        public bool Equals(IIOArray obj) => Equals(obj as Vector4);
        public Vector4 FromArray(double[] array, int offset = 0)
        {
            this.X = array[offset];
            this.Y = array[offset + 1];
            this.Z = array[offset + 2];
            this.W = array[offset + 3];
            return this;
        }
        IIOArray IIOArray.FromArray(double[] array, int offset)
        {
            return FromArray(array, offset);
        }
        public double[] ToArray(double[] array = null, int offset = 0)
        {
            if (array == null) array = new double[4];
            array[offset] = this.X;
            array[offset + 1] = this.Y;
            array[offset + 2] = this.Z;
            array[offset + 3] = this.W;
            return array;
        }
        public Vector4 Random()
        {
            this.X = MathEx.Random();
            this.Y = MathEx.Random();
            this.Z = MathEx.Random();
            this.W = MathEx.Random();
            return this;
        }
        #endregion

        public IEnumerator GetEnumerator()
        {
            yield return this.X;
            yield return this.Y;
            yield return this.Z;
            yield return this.W;
        }

        public override bool Equals(Vector obj)
        {
            return this.Equals(obj as Vector4);
        }
    }
}
